﻿
// 2018/8/23: 首个记录，基于V2 2018/7/3
// 2018/8/24: fix GetGeneralSampleProtocols
// 2018/9/17: 计算DTLC、STLC、TTLC时 isLeft 参数重新定义
// 2018/11/29: 增加Protocols静态属性
// 2019/3/21: 实现ToGeneralSample。时间戳赋值
// 2019/3/27: ToGeneralSample协议赋值
// 2019/3/29: 添加注释
// 2019/9/26: 车道线颜色扩充
// 2019/9/27: 车道线颜色扩充2

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using ASEva;

namespace LaneSensorSampleV3
{
    enum LineClass
    {
        General = 1, // 未指定
        Dashed = 2, // 虚线
        Solid = 3, // 实线
        DoubleDashed = 4, //双虚线
        DoubleSolid = 5, //双实线
        SolidDashed = 6, // 左实右虚
        DashedSolid = 7, // 左虚右实
        Curb = 8, // 路沿
        Guardrail = 9, // 护栏
        Barrier = 10, //障碍物边界
    }

    enum LineColor
    {
        Unknown = 1, // 未知
        White = 2, // 白色
        Yellow = 3, // 黄色
        Red = 4, // 红色
        Blue = 5, // 蓝色
        Orange = 6, // 橙色
        Green = 7, // 绿色
    }

    class LaneLine
    {
        // 主要部分
        public int ID { get; set; } // 车道线ID
        public int? PublicID { get; set; } // 车道线原始ID
        public LineClass LineClass { get; set; } // 车道线类型
        public LineColor Color { get; set; } // 车道线颜色
        public double? Confidence { get; set; } // 车道线置信度 %
        public double? Width { get; set; } // 车道线宽度 m
        public double[] ModelCoefs { get; set; } // y = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3
        public double? RearEnd { get; set; } // 曲线后端点位置(x轴) m
        public double? FrontEnd { get; set; } // 曲线前端点位置(x轴) m

        // 指标部分
        public double? DTLC { get; set; } // m
        public double? STLC { get; set; } // m/s
        public double? TTLC { get; set; } // s

        public LaneLine()
        {
            ID = -1;
            LineClass = LineClass.General;
            Color = LineColor.Unknown;
            ModelCoefs = new double[] { 0, 0, 0, 0 };
        }
    }

    class LaneSensorSample : Sample
    {
        // 车道线
        public List<LaneLine> Lines { get; set; } // 车道线列表
        public int? FirstLeftLineIndex { get; set; } // 左侧第一条车道线序号
        public int? FirstRightLineIndex { get; set; } // 右侧第一条车道线序号
        public int? SecondLeftLineIndex { get; set; } // 左侧第二条车道线序号
        public int? SecondRightLineIndex { get; set; } // 右侧第二条车道线序号

        // 本车道行驶状态
        public double? LateralVelocity { get; set; } // 当前车道横向速度 m/s
        public double? CenterDeparture { get; set; } // 当前车道中央偏离距离 m

        // 道路属性部分
        public double? LaneWidth { get; set; } // 当前车道宽度 m
        public double? LaneHeading { get; set; } // 当前车道朝向 deg
        public double? LaneCurvature { get; set; } // 当前车道曲率 1/m

        // 车辆参数部分
        public double? VehicleSpeed { get; set; } // 本车车速 kph
        public double? VehicleCurvature { get; set; } // 本车行驶曲率 1/m
        public double? VehicleAX { get; set; } // 本车纵向加速度 m/s²
        public double? VehicleWidth { get; set; } // 本车宽度 m
        public double? VehicleFrontOverhang { get; set; } // 本车前悬 m

        // 车道线识别边界
        public double LinesRearBound { get; set; } // 传感器检测范围后边界 m
        public double LinesFrontBound { get; set; } // 传感器检测范围前边界 m

        public LaneSensorSample()
        {
            Lines = new List<LaneLine>();
            LinesFrontBound = 30;
        }

        public override bool SupportInterpolation()
        {
            return true;
        }

        public override string GetGeneralSampleProtocol()
        {
            return "lane-sensor-sample-v3";
        }

        public static string[] Protocols
        {
            get
            {
                return new string[] { "lane-sensor-sample-v2", "lane-sensor-sample-v3" };
            }
        }
        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }

        public override bool FromGeneralSample(GeneralSample sample)
        {
            if (sample.Protocol == "lane-sensor-sample-v3")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 17) return false;
                if (v[0].IsNotNumber()) return false;

                int lineCount = (int)v[0].number;
                if (v.Length != 17 + lineCount * 15) return false;

                LateralVelocity = v[1].ToDouble();
                CenterDeparture = v[2].ToDouble();
                LaneWidth = v[3].ToDouble();
                LaneHeading = v[4].ToDouble();
                LaneCurvature = v[5].ToDouble();
                VehicleSpeed = v[6].ToDouble();
                VehicleCurvature = v[7].ToDouble();
                VehicleAX = v[8].ToDouble();
                VehicleWidth = v[9].ToDouble();
                VehicleFrontOverhang = v[10].ToDouble();
                LinesRearBound = v[11].number;
                LinesFrontBound = v[12].number;
                FirstLeftLineIndex = v[13].ToInt();
                FirstRightLineIndex = v[14].ToInt();
                SecondLeftLineIndex = v[15].ToInt();
                SecondRightLineIndex = v[16].ToInt();

                Lines.Clear();
                for (int i = 0; i < lineCount; i++)
                {
                    var line = new LaneLine();
                    int baseIndex = 17 + 15 * i;

                    line.ID = (int)v[baseIndex + 0].number;
                    line.PublicID = v[baseIndex + 1].ToInt();
                    line.LineClass = (LineClass)(int)v[baseIndex + 2].number;
                    line.Color = (LineColor)(int)v[baseIndex + 3].number;
                    line.Confidence = v[baseIndex + 4].ToDouble();
                    line.Width = v[baseIndex + 5].ToDouble();
                    line.ModelCoefs[0] = v[baseIndex + 6].number;
                    line.ModelCoefs[1] = v[baseIndex + 7].number;
                    line.ModelCoefs[2] = v[baseIndex + 8].number;
                    line.ModelCoefs[3] = v[baseIndex + 9].number;
                    line.RearEnd = v[baseIndex + 10].ToDouble();
                    line.FrontEnd = v[baseIndex + 11].ToDouble();
                    line.DTLC = v[baseIndex + 12].ToDouble();
                    line.STLC = v[baseIndex + 13].ToDouble();
                    line.TTLC = v[baseIndex + 14].ToDouble();

                    Lines.Add(line);
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }
            else if (sample.Protocol == "lane-sensor-sample-v2")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 30) return false;

                var leftDetected = v[0].number == 1;
                var rightDetected = v[1].number == 1;

                Lines.Clear();

                int index = 0;
                if (leftDetected)
                {
                    var newLine = new LaneLine();

                    newLine.ModelCoefs = new double[4]
                    {
                        v[2].number,
                        v[3].number,
                        v[4].number,
                        v[5].number,
                    };
                    newLine.LineClass = (LineClass)v[6].number;

                    newLine.DTLC = v[17].ToDouble();
                    newLine.TTLC = v[18].ToDouble();
                    newLine.STLC = v[19].ToDouble();

                    Lines.Add(newLine);
                    FirstLeftLineIndex = index++;
                }
                if (rightDetected)
                {
                    var newLine = new LaneLine();

                    newLine.ModelCoefs = new double[4]
                    {
                        v[8].number,
                        v[9].number,
                        v[10].number,
                        v[11].number,
                    };
                    newLine.LineClass = (LineClass)v[12].number;

                    newLine.DTLC = v[20].ToDouble();
                    newLine.TTLC = v[21].ToDouble();
                    newLine.STLC = v[22].ToDouble();

                    Lines.Add(newLine);
                    FirstRightLineIndex = index++;
                }

                LaneWidth = v[14].ToDouble();
                LaneHeading = v[15].ToDouble();
                LaneCurvature = v[16].ToDouble();

                LateralVelocity = v[23].ToDouble();
                CenterDeparture = v[24].ToDouble();

                VehicleSpeed = v[25].ToDouble();
                VehicleCurvature = v[26].ToDouble();
                VehicleAX = v[27].ToDouble();
                VehicleWidth = v[28].ToDouble();
                VehicleFrontOverhang = v[29].ToDouble();

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }

            return false;
        }

        public override GeneralSample ToGeneralSample()
        {
            var sample = new GeneralSample();
            sample.Base = Base;
            sample.Offset = Offset;
            sample.Timeline = Timeline;
            sample.Protocol = GetGeneralSampleProtocol();

            var data = new GeneralSampleValue[17 + Lines.Count * 15];
            data[0] = new GeneralSampleValue((double)Lines.Count);

            if (LateralVelocity != null) data[1] = new GeneralSampleValue(LateralVelocity.Value);
            if (CenterDeparture != null) data[2] = new GeneralSampleValue(CenterDeparture.Value);
            if (LaneWidth != null) data[3] = new GeneralSampleValue(LaneWidth.Value);
            if (LaneHeading != null) data[4] = new GeneralSampleValue(LaneHeading.Value);
            if (LaneCurvature != null) data[5] = new GeneralSampleValue(LaneCurvature.Value);
            if (VehicleSpeed != null) data[6] = new GeneralSampleValue(VehicleSpeed.Value);
            if (VehicleCurvature != null) data[7] = new GeneralSampleValue(VehicleCurvature.Value);
            if (VehicleAX != null) data[8] = new GeneralSampleValue(VehicleAX.Value);
            if (VehicleWidth != null) data[9] = new GeneralSampleValue(VehicleWidth.Value);
            if (VehicleFrontOverhang != null) data[10] = new GeneralSampleValue(VehicleFrontOverhang.Value);
            data[11] = new GeneralSampleValue(LinesRearBound);
            data[12] = new GeneralSampleValue(LinesFrontBound);
            if (FirstLeftLineIndex != null) data[13] = new GeneralSampleValue((double)FirstLeftLineIndex.Value);
            if (FirstRightLineIndex != null) data[14] = new GeneralSampleValue((double)FirstRightLineIndex.Value);
            if (SecondLeftLineIndex != null) data[15] = new GeneralSampleValue((double)SecondLeftLineIndex.Value);
            if (SecondRightLineIndex != null) data[16] = new GeneralSampleValue((double)SecondRightLineIndex.Value);

            for (int i = 0; i < Lines.Count; i++)
            {
                LaneLine src = Lines[i];
                int baseIndex = 17 + 15 * i;

                data[baseIndex + 0] = new GeneralSampleValue((double)src.ID);
                if (src.PublicID != null) data[baseIndex + 1] = new GeneralSampleValue((double)src.PublicID.Value);
                data[baseIndex + 2] = new GeneralSampleValue((double)(uint)src.LineClass);
                data[baseIndex + 3] = new GeneralSampleValue((double)(uint)src.Color);
                if (src.Confidence != null) data[baseIndex + 4] = new GeneralSampleValue(src.Confidence.Value);
                if (src.Width != null) data[baseIndex + 5] = new GeneralSampleValue(src.Width.Value);
                data[baseIndex + 6] = new GeneralSampleValue(src.ModelCoefs[0]);
                data[baseIndex + 7] = new GeneralSampleValue(src.ModelCoefs[1]);
                data[baseIndex + 8] = new GeneralSampleValue(src.ModelCoefs[2]);
                data[baseIndex + 9] = new GeneralSampleValue(src.ModelCoefs[3]);
                if (src.RearEnd != null) data[baseIndex + 10] = new GeneralSampleValue(src.RearEnd.Value);
                if (src.FrontEnd != null) data[baseIndex + 11] = new GeneralSampleValue(src.FrontEnd.Value);
                if (src.DTLC != null) data[baseIndex + 12] = new GeneralSampleValue(src.DTLC.Value);
                if (src.STLC != null) data[baseIndex + 13] = new GeneralSampleValue(src.STLC.Value);
                if (src.TTLC != null) data[baseIndex + 14] = new GeneralSampleValue(src.TTLC.Value);
            }

            sample.Values = data.ToList();
            sample.NumberOfSignificants = sample.Values.Count;

            return sample;
        }

        protected override Sample Interpolate(SearchResult input)
        {
            var s1 = input.s1 as LaneSensorSample;
            var s2 = input.s2 as LaneSensorSample;
            var w1 = input.w1;
            var w2 = input.w2;

            bool noID = false;
            foreach (var line in s1.Lines)
            {
                if (line.ID < 0)
                {
                    noID = true;
                    break;
                }
            }
            foreach (var line in s2.Lines)
            {
                if (line.ID < 0)
                {
                    noID = true;
                    break;
                }
            }

            if (noID)
            {
                return w1 > w2 ? s1 : s2;
            }

            int? firstLeftID = null, firstRightID = null;
            if (s1.FirstLeftLineIndex != null && s2.FirstLeftLineIndex != null &&
                s1.Lines[s1.FirstLeftLineIndex.Value].ID == s2.Lines[s2.FirstLeftLineIndex.Value].ID)
            {
                firstLeftID = s1.Lines[s1.FirstLeftLineIndex.Value].ID;
            }
            if (s1.FirstRightLineIndex != null && s2.FirstRightLineIndex != null &&
                s1.Lines[s1.FirstRightLineIndex.Value].ID == s2.Lines[s2.FirstRightLineIndex.Value].ID)
            {
                firstRightID = s1.Lines[s1.FirstRightLineIndex.Value].ID;
            }

            int? secondLeftID = null, secondRightID = null;
            if (s1.SecondLeftLineIndex != null && s2.SecondLeftLineIndex != null &&
                s1.Lines[s1.SecondLeftLineIndex.Value].ID == s2.Lines[s2.SecondLeftLineIndex.Value].ID)
            {
                secondLeftID = s1.Lines[s1.SecondLeftLineIndex.Value].ID;
            }
            if (s1.SecondRightLineIndex != null && s2.SecondRightLineIndex != null &&
                s1.Lines[s1.SecondRightLineIndex.Value].ID == s2.Lines[s2.SecondRightLineIndex.Value].ID)
            {
                secondRightID = s1.Lines[s1.SecondRightLineIndex.Value].ID;
            }

            var output = new LaneSensorSample();

            output.LateralVelocity = s1.LateralVelocity * w1 + s2.LateralVelocity * w2;
            output.CenterDeparture = s1.CenterDeparture * w1 + s2.CenterDeparture * w2;

            output.LaneWidth = s1.LaneWidth * w1 + s2.LaneWidth * w2;
            output.LaneCurvature = s1.LaneCurvature * w1 + s2.LaneCurvature * w2;
            output.LaneHeading = s1.LaneHeading * w1 + s2.LaneHeading * w2;

            output.VehicleSpeed = s1.VehicleSpeed * w1 + s2.VehicleSpeed * w2;
            output.VehicleCurvature = s1.VehicleCurvature * w1 + s2.VehicleCurvature * w2;
            output.VehicleAX = s1.VehicleAX * w1 + s2.VehicleAX * w2;
            output.VehicleWidth = s1.VehicleWidth;
            output.VehicleFrontOverhang = s1.VehicleFrontOverhang;

            output.LinesRearBound = w1 > w2 ? s1.LinesRearBound : s2.LinesRearBound;
            output.LinesFrontBound = w1 > w2 ? s1.LinesFrontBound : s2.LinesFrontBound;

            var lines = new List<LaneLine>();
            int index = 0;
            for (int i = 0; i < s1.Lines.Count; i++)
            {
                var s1Line = s1.Lines[i];
                for (int j = 0; j < s2.Lines.Count; j++)
                {
                    var s2Line = s2.Lines[j];

                    if (s1Line.ID == s2Line.ID)
                    {
                        if (firstLeftID != null && firstLeftID.Value == s1Line.ID) output.FirstLeftLineIndex = index;
                        if (firstRightID != null && firstRightID.Value == s1Line.ID) output.FirstRightLineIndex = index;
                        if (secondLeftID != null && secondLeftID.Value == s1Line.ID) output.SecondLeftLineIndex = index;
                        if (secondRightID != null && secondRightID.Value == s1Line.ID) output.SecondRightLineIndex = index;

                        var line = new LaneLine();
                        line.ID = s1Line.ID;
                        line.PublicID = s1Line.PublicID;
                        line.LineClass = w1 > w2 ? s1Line.LineClass : s2Line.LineClass;
                        line.Color = w1 > w2 ? s1Line.Color : s2Line.Color;
                        line.Confidence = s1Line.Confidence * w1 + s2Line.Confidence * w2;
                        line.Width = s1Line.Width * w1 + s2Line.Width * w2;
                        line.ModelCoefs[0] = s1Line.ModelCoefs[0] * w1 + s2Line.ModelCoefs[0] * w2;
                        line.ModelCoefs[1] = s1Line.ModelCoefs[1] * w1 + s2Line.ModelCoefs[1] * w2;
                        line.ModelCoefs[2] = s1Line.ModelCoefs[2] * w1 + s2Line.ModelCoefs[2] * w2;
                        line.ModelCoefs[3] = s1Line.ModelCoefs[3] * w1 + s2Line.ModelCoefs[3] * w2;
                        line.RearEnd = s1Line.RearEnd * w1 + s2Line.RearEnd * w2;
                        line.FrontEnd = s1Line.FrontEnd * w1 + s2Line.FrontEnd * w2;
                        lines.Add(line);
                        index++;
                        break;
                    }
                }
            }
            output.Lines = lines;

            output.RecalculateIndicators();

            return output;
        }

        public void RecalculateIndicators()
        {
            if (VehicleWidth != null && VehicleFrontOverhang != null)
            {
                foreach (var line in Lines)
                {
                    line.DTLC = calDTLC(VehicleWidth.Value, VehicleFrontOverhang.Value, line.ModelCoefs);
                }
            }
            if (VehicleSpeed != null)
            {
                foreach (var line in Lines)
                {
                    line.STLC = calSTLC(VehicleSpeed.Value, line.ModelCoefs);
                }
            }
            if (VehicleSpeed != null && VehicleWidth != null && VehicleFrontOverhang != null)
            {
                foreach (var line in Lines)
                {
                    line.TTLC = calTTLC(VehicleSpeed.Value, VehicleWidth.Value, VehicleFrontOverhang.Value, VehicleAX, VehicleCurvature, line.ModelCoefs);
                }
            }
        }

        private double? calDTLC(double vehiWidth, double vehiFrontOH, double[] values)
        {
            bool isLeft = values[0] > 0;

	        double x = -1 * vehiFrontOH;
	        double y = (values[3] * x * x * x + values[2] * x * x
		        + values[1] * x + values[0]) * (isLeft ? 1 : -1);
	        double cosTheta = Math.Abs(Math.Cos(Math.Atan(values[1])));
	        return cosTheta * (y - vehiWidth / 2.0);
        }

        private double? calSTLC(double vehiVX, double[] values)
        {
            bool isLeft = values[0] > 0;

            Double sinTheta = Math.Sin(Math.Atan(values[1]));
	        return (isLeft ? -1.0 : 1.0) * sinTheta * vehiVX / 3.6;
        }

        private double? calTTLC(double vx, double width, double overhang, double? ax, double? curv, double[] values)
        {
            bool isLeft = values[0] > 0;

	        double x = 0;
	        double y = 0;
	        vx /= 3.6; // 转成m/s2

	        for (int i = 0; i <= 1000; i++) // TTLC最大10秒
	        {
		        double tx = x - overhang;
		        double ty = y + (isLeft ? 0.5 : -0.5) * width;
		        double ly = values[3] * tx * tx * tx + values[2] * tx * tx + values[1] * tx + values[0];

		        bool passed = false;
		        if (isLeft)
		        {
			        if (ly <= ty) passed = true;
		        }
		        else
		        {
			        if (ly >= ty) passed = true;
		        }
		        if (passed)
		        {
			        if (i == 0) return null;
			        else return (double)i / 100;
		        }

		        Double k = 0;
		        if (curv != null) k = curv.Value * x;

		        if (ax != null) vx += ax.Value * 0.01;
		        if (vx <= 0) break;

		        x += vx * 0.01 / Math.Abs(1 + k);
                y += vx * 0.01 * k / Math.Abs(1 + k);
	        }

            return null;
        }
    }
}
